#include <cstdio> // debugging

#include "Viewer.h"
#include "Spring.h"

Viewer::Viewer(const char* _title, int _width, int _height,
		double _timeStep, double _grav, int numIt) :
	TrackballViewer(_title, _width, _height)
{
	collisionSystem = new CollisionSystem(
		AABB(Vector3(-10, -10, -10), Vector3(10, 10, 10)));

	particleSystem = new ParticleSystem();
	particleSystem->timeStep = _timeStep;
	particleSystem->gravitationalAcceleration = Vector3(0, _grav, 0);
	particleSystem->numIterations = numIt;
	particleSystem->collisionSystem = collisionSystem;

	bridge = new Bridge();
	particleSystem->addObject(bridge);
	collisionSystem->addObject(bridge);

	box1 = new Box(AABB(Vector3(0, 0.2, 0), Vector3(-0.3, 0.4, 0.2)), 1, 1);
	particleSystem->addObject(box1);
	collisionSystem->addObject(box1);

	/*
	box2 = new Box(AABB(Vector3(0.1, 0.31, 0), Vector3(0.3, 0.51, 0.2)), 1, 1);
	particleSystem->addObject(box2);
	collisionSystem->addObject(box2);
	*/

	frame = curTime = oldTime = 0;
	init();
}

//-----------------------------------------------------------------------------

Viewer::~Viewer()
{
	delete box1;
	delete bridge;
	delete particleSystem;
	delete collisionSystem;
}

//-----------------------------------------------------------------------------

void Viewer::init()
{
	TrackballViewer::init();
	set_scene_pos(Vector3(0.0, 0.0, 0.0), 1.0);
}

//-----------------------------------------------------------------------------

void Viewer::draw_scene(DrawMode _drawMode)
{
	glEnable(GL_DEPTH_TEST);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glDisable(GL_CULL_FACE);

	// Camera
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	// 	projection matrix
	Matrix4 m = (m_camera.getProjectionMatrix() * m_camera.getTransformation().Inverse()).Transpose();
	glMultMatrixd(m.dataBlock());
	
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	glBegin(GL_LINES);
	set<Constraint*>::iterator c;
	for(c = bridge->constraint.begin(); c != bridge->constraint.end(); ++c) {
		Spring* s = (Spring *)(*c);
		Vector3& p1 = s->particle1->position;
		Vector3& p2 = s->particle2->position;
		glVertex3f(p1.x, p1.y, p1.z);
		glVertex3f(p2.x, p2.y, p2.z);
	}
	for(c = box1->constraint.begin(); c != box1->constraint.end(); ++c) {
		Spring* s = (Spring *)(*c);
		Vector3& p1 = s->particle1->position;
		Vector3& p2 = s->particle2->position;
		glVertex3f(p1.x, p1.y, p1.z);
		glVertex3f(p2.x, p2.y, p2.z);
	}
	/*
	for(c = box2->constraint.begin(); c != box2->constraint.end(); ++c) {
		Spring* s = (Spring *)(*c);
		Vector3& p1 = s->particle1->position;
		Vector3& p2 = s->particle2->position;
		glVertex3f(p1.x, p1.y, p1.z);
		glVertex3f(p2.x, p2.y, p2.z);
	}
	*/
	glEnd();

	glPointSize(2);
	glBegin(GL_POINTS);
	set<Particle*>::iterator p;
	for(p = bridge->particle.begin(); p != bridge->particle.end(); ++p) {
		Vector3& pos = (*p)->position;
		glVertex3f(pos.x, pos.y, pos.z);
	}
	for(p = box1->particle.begin(); p != box1->particle.end(); ++p) {
		Vector3& pos = (*p)->position;
		glVertex3f(pos.x, pos.y, pos.z);
		//fprintf(stderr, "%.3lf %.3lf %.3lf\n", pos.x, pos.y, pos.z);
	}
	/*
	for(p = box2->particle.begin(); p != box2->particle.end(); ++p) {
		Vector3& pos = (*p)->position;
		glVertex3f(pos.x, pos.y, pos.z);
		//fprintf(stderr, "%.3lf %.3lf %.3lf\n", pos.x, pos.y, pos.z);
	}
	*/
	glEnd();
}

//-----------------------------------------------------------------------------

void Viewer::idle()
{
	// Update the physics simulation
	particleSystem->update();
	glutPostRedisplay();

	// Compute the framerate
	++frame;
	curTime = glutGet(GLUT_ELAPSED_TIME); // time in miliseconds
	if(curTime-oldTime > 5000) {
		fprintf(stderr, "FPS %4.2f\n", frame*1000.0/(curTime-oldTime));
		oldTime = curTime;
		frame = 0;
	}
}

//-----------------------------------------------------------------------------

/*
void Viewer::mouse(int button, int state, int x, int y)
{
	// mouse press
	if (state == GLUT_DOWN)
	{
		last_x_ = x;
		last_y_ = y;
		last_point_ok_ = map_to_sphere( x, y, last_point_3D_ );
		button_down_[button] = true;
	}


	// mouse release
	else
	{
		last_point_ok_ = false;
		button_down_[button] = false;

		// GLUT: button 3 or 4 -> mouse wheel clicked
		if (button == 3)       
			zoom(0, (int)(y - 0.05*width_));
		else if (button == 4)
			zoom(0, (int)(y + 0.05*width_));
	}

	modifiers_ = glutGetModifiers();

	glutPostRedisplay();
}
*/

//-----------------------------------------------------------------------------

void Viewer::motion(int x, int y)
{
	// zoom is handled by TrackballViewer::motion()
	if(button_down_[1]) {
		translation(x, y);
	} else if(button_down_[2]) {
		rotation(x, y);
	}

	// remeber points
	last_x_ = x;
	last_y_ = y;
	last_point_ok_ = map_to_sphere(x, y, last_point_3D_);

	glutPostRedisplay();
}
